#pragma once
#ifndef _MATH_UTILS_H
#define _MATH_UTILS_H

#include <map>
#include <math.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <sophus/se3.hpp>
#include <sophus/sim3.hpp>

using namespace std;
using namespace Eigen;

typedef Matrix<double, 6, 1> Vector6d;
typedef Matrix<double, 6, 6> Matrix6d;
typedef Matrix<double, 7, 7> Matrix7d;
typedef Matrix<double, 7, 1> Vector7d;
typedef Matrix<double, 13, 13> Matrix13d;
typedef Matrix<double, 13, 1> Vector13d;

const double deg2rad = M_PI/180;

//x ~= 2nPI
bool IS_N2PI(double x, double th);
//x ~= 2nPI +- PI
bool IS_N2PI_PI(double x, double th);


enum Frank_METHOD {
  DT_T_SIM = 0,
  DT_FIRST,
  T_FIRST,
  GAUSS_NEWTON,
};

Sophus::SE3d FrankOpt(
       map<int, Sophus::SE3d> &pt,
       map<int, Sophus::SE3d> &ps,
       Matrix3d& R_, Vector3d& t_, double &s_, int &S_, float wr,
       bool IS_ALIGN_SCALE,
       Frank_METHOD solveMethod);

void pose_estimation_3d3d_use_Umeyama(
       map<int, Sophus::SE3d> &pt,
       map<int, Sophus::SE3d> &ps,
       //map<int, Vector3d> &pt,
       //map<int, Vector3d> &ps,
       Matrix3d& R_, Vector3d& t_, double &s_, int &S_,
       bool IS_ALIGN_SCALE);

Sophus::Sim3d pose_estimation_3d3d_use_salas(
       map<int, Sophus::SE3d> &pt,
       map<int, Sophus::SE3d> &ps,
       float wr,
       bool IS_ROT_TRANSPOSE,
       bool IS_ALIGN_SCALE);

void pose_estimation_3d3d(
       map<int, Sophus::SE3d> &pt,
       map<int, Sophus::SE3d> &ps,
       //map<int, Vector3d> &pt,
       //map<int, Vector3d> &ps,
       Matrix3d& R_, Vector3d& t_, double &s_, int &S_,
       bool IS_ALIGN_SCALE);

//Params: (pt,ps,s,S,R,t), Pt = s*SR*ps + St; Rt1^T * Rt = SR *Rs1^T *Rs* SR
void pose_estimation_3d3d_with_rot(
       map<int, Sophus::SE3d> &pt,
       map<int, Sophus::SE3d> &ps,
       Matrix3d& R_, Vector3d& t_, double &s_, int &S_,
       bool IS_ALIGN_SCALE,
       float wr,
       bool IS_ROT_TRANSPOSE);

Quaterniond Quaternion_S_lerp(Quaterniond &start_q,
       Quaterniond &end_q, double c);

Sophus::SE3d se_lerp(Sophus::SE3d se1, Sophus::SE3d se2, int c);

#endif //_MATH_UTILS_H
